// Copyright [year] <Copyright Owner>
#include <sys/ioctl.h>
#include <stdio.h>      /*标准输入输出定义*/
#include <stdlib.h>     /*标准函数库定义*/
#include <unistd.h>     /*Unix标准函数定义*/
#include <sys/types.h>  /**/
#include <sys/stat.h>   /**/
#include <fcntl.h>      /*文件控制定义*/
//#include <termios.h>    /*PPSIX终端控制定义*/
#include <errno.h>      /*错误号定义*/
#include <linux/serial.h>
#include <string.h>
#include <stdint.h>
//#include <asm/termios.h>
#include <asm-generic/termbits.h>

#include <iostream>
#include <queue>
#include <thread>

#include "frame_paser.hpp"

#define BUFSIZE 2000
std::vector<uint8_t> data_vector;
/* 帧示例说明：
编号  字节数  功能      备注
1      2    帧头标识   0xEB 0x90
2      1    帧长度     从编号 4~结束符的总字节个数
3      1    命令字     0x01
4      24   位置数据   3*double，经纬高
5      12   速度数据   3*float，XYZ
6      12   姿态数据   3*float，ZXY 欧拉角
7      2    结束符     0x2A00
8      1    累加校验   编号 2~结束符累加
*/
typedef struct {
    double pos[3];  // 位置数据
    float spe[3];  // 速度数据
    float att[3];  // 姿态数据
} PVQ_T;
PVQ_T PVQ;

typedef struct {
    float rate[3];
    float acc[3];
    float rate0[3];
    float acc0[3];
} INS_T;
INS_T INS;

typedef struct {
    double gnss_pos[3];
    float gnss_rete[2];
    float course;
    uint8_t stategps;
    uint8_t stateheading;
} GNSS_T;
GNSS_T GNSS;

typedef struct {
    float AntDistance;
    float dyawGNSS;
    float GNSSxyz[3];
    float IMUxyz[3];
} VEHI_T;
VEHI_T VEHI;

typedef struct {
    int8_t active_state;
    char version[20];
    char DEVID[12];
    char ICCID[20];
} MSG_T;
MSG_T MSG;

void processing(uint8_t * data, uint32_t data_len) {
  if (data[0] == 0xEB && data[1] == 0x90) {
    if (data_len >= 7) {
      if (data[3] == 0x01) {
        memcpy(PVQ.pos, &data[4], 24);
        memcpy(PVQ.spe, &data[28], 12);
        memcpy(PVQ.att, &data[40], 12);
        // printf("pvq %f, %f, %f\n", PVQ.pos[0], PVQ.pos[1], PVQ.pos[2]);
      } else if (data[3] == 0x02) {
        memcpy(INS.rate, &data[4], 12);
        memcpy(INS.acc, &data[16], 12);
        memcpy(INS.rate0, &data[28], 12);
        memcpy(INS.acc0, &data[40], 12);
      } else if (data[3] == 0x03) {
        memcpy(GNSS.gnss_pos, &data[4], 24);
        memcpy(GNSS.gnss_rete, &data[28], 8);
        memcpy(&GNSS.course, &data[36], 4);
        memcpy(&GNSS.stategps, &data[40], 1);
        memcpy(&GNSS.stateheading, &data[41], 1);
        // printf("gnss %f, %f, %f\n", GNSS.gnss_pos[0], GNSS.gnss_pos[1], GNSS.gnss_pos[2]);
      } else if (data[3] == 0x05) {
        memcpy(&MSG.active_state, &data[36], 1);
        memcpy(MSG.version, &data[38], 20);
        memcpy(MSG.DEVID, &data[4], 12);
        memcpy(MSG.ICCID, &data[16], 20);
      } else if (data[3] == 0x07) {
        memcpy(&VEHI.AntDistance, &data[4], 4);
        memcpy(&VEHI.dyawGNSS, &data[8], 4);
        memcpy(VEHI.GNSSxyz, &data[12], 12);
        memcpy(VEHI.IMUxyz, &data[24], 12);
      }
    }
  }
}

int uart_init(int fd, int speed) {
  struct termios2 tio;

  tio.c_cflag = BOTHER | CS8 | CLOCAL | CREAD;
  tio.c_iflag = IGNPAR;
  tio.c_oflag = 0;
  tio.c_ispeed = speed;
  tio.c_ospeed = speed;
  return ioctl(fd, TCSETS2, &tio);
}
#if 0
std::vector<uint8_t> data_used_vector;
void getFullFrame() {
  while (true) {
    if (data_vector.empty()) {
    continue;
    }
    int len = data_vector.size();
    for (int i = 0; i < len; i++) {
      if (data_vector[i] == 0xeb) {
      }
    }
    if (data.front() != 0xeb) {
      data.pop();
    } else {
      if (!data.empty()) {
        uint8_t header_byte0 = data.front();  // 0xeb
        data.pop();  // pop 0xeb
        uint8_t header_byte1 = data.front();
        if (header_byte1 == 0x90) {
          data.pop();  // pop 0x90
          data_used_vector.emplace_back(header_byte0);
          data_used_vector.emplace_back(header_byte1);
          uint8_t frame_len = data.front();
          int queue_size = data.size();
          if (queue_size < frame_len) {
            for (int i = 0; i < queue_size; i++) {
              data_used_vector.emplace_back(data.front());
              data.pop();
            }
          } else {}
        }
      }
    }
  }
}
#endif
  FrameParser<std::uint8_t> parser{{ 0xeb, 0x90 }, { 0x2a, 0x00 }, false};
  // std::vector<std::vector<std::uint8_t>> frames;
/* 参数 t: 查找源内容
   参数 s: 查找的内容
   参数 pos: 找到s在t中第一次出现的位置索引
   返回值: true 在t中找到 s
          false  在t中没找到 s
*/
bool isSubvector(std::vector<unsigned char> s, std::vector<unsigned char> t, int *pos) {
  if (t.empty() && !s.empty()) return false;
  if (s.empty()) return true;
  if (t.size() < s.size()) return false;
  // cout << "s.size = " << s.size() << endl;
  // cout << "t.size = " << t.size() << " t[7] = " << t[7] << endl;
  for (int i = 0; i < t.size() - s.size() + 1; i++) {
    if (t[i] == s[0]) {  // 找到起始字符
      int j = i + 1;  // 指示字符串t中字符位置的指针
      int k = 1;      // 指示字符串s中字符位置的指针
      while (j < t.size()) {
        if (t[j] == s[k]) {   // 找到一个相同字符，那就将指针位置都加1
          j++;
          k++;
        } else {
            j++;   // 没找到，继续看看字符串t中后面的字符
        }
        if (k == s.size()) {
          // cout << "j - 3 = " << j -3 << " " << t[j-3]<< endl;
          *pos = j - s.size();
          return true;  // 找完整个字符串s了，提前结束，返回true
        }
      }
    }
  }
  return false;
}

bool isSubsequence(string s, string t) {
  int is = 0, it = 0;
  while (is < s.size() && it < t.size()) {
    if (s[is] == t[it]) {
      is++;
    }
    it++;
  }
  if(is == s.size()) 
    return true;
  else 
    return false;
}

void calc_chksum(std::vector<std::uint8_t> data) {
  uint8_t chksum = 0;
  if (data.size() > 7) {
    for (auto i = 4; i < data.size() - 5; i++) {
      chksum += data[i];
    }
    if (chksum == data[data.size() - 1]) {
      // std::cout << "chksum is the same as in frame" << std::endl;
    }
    if (data[2] == data.size() - 5) {
      // std::cout << "frame len  is the same as in frame len " << std::endl;
    }
  }
}

void process(int uart_fd) {
  // FrameParser<std::uint8_t> parser{{ 0xeb, 0x90 }, { 0x2a, 0x00 }, false};
  std::vector<std::vector<std::uint8_t>> frames;
  while (1) {
    uint8_t tmp_buf[BUFSIZE];

    int uart_len = read(uart_fd, tmp_buf, BUFSIZE);
    if (uart_len < 0) {
        printf("read uart failed\n");
        exit(1);
    }
    if (uart_len != 0) {
      for (int i = 0; i < uart_len; i++) {
        data_vector.emplace_back(tmp_buf[i]);
      }
      if (uart_len == 0) printf("receive 0 byte\n");
      // printf("data_vector.size = %ld\n", data_vector.size());

      /*if (fid != nullptr) {
        fwrite(tmp_buf, 1, uart_len, fid);
      } else {
        printf("failed to write file\n");
      }*/
      parser.append(data_vector);
      parser.parse(frames);
      // printf("frame.size = %lu\n", frames.size());

      // for (auto data : frames) {
      //   for (auto vv : data) {
      //       printf("0x%02x ", vv);
      //   }
      //   printf("\n");
      // }

      // processing(frames.data()->data(), frames.data()->size());
      // std::cout << "frames nums = " << frames.size() << std::endl;
      // printf("%s \n", MSG.version);
      // for (auto i = 0; i < data_vector.size(); i++)
      //   printf("0x%02x ", data_vector[i]);
      // printf("\n");
      // printf("data_vector.size = %ld\n", data_vector.size());
      // printf("--------------\n");
      int pos = 0;
      for (auto i = 0; i < frames.size(); i++) {
        auto iter = find_first_of(data_vector.begin(), data_vector.end(), frames[i].begin(), frames[i].end());
        if (iter != data_vector.end()) {
          if (isSubvector(frames[i], data_vector, &pos)) {
            std::vector<std::uint8_t> tmp(data_vector.begin() + pos, data_vector.begin() + pos + frames[i].size() + 1);
            // std::cout << tmp.size() << std::endl;
            // for (auto i = 0; i < tmp.size(); i++)
            //   printf("0x%02x ", tmp[i]);
            // printf("\n");
            calc_chksum(tmp);
            data_vector.erase(data_vector.begin() + pos, data_vector.begin() + pos + frames[i].size() + 1);
          }
        } else {
          std::cout << "Not found!" << std::endl;
        }
      }

      // uint8_t chk_sum = 0;
      // for (auto i = 0; i < frames.size(); i ++) {
      //    std::uint8_t *data = frames[i].data();
      //    if (data[2] == frames[i].size() - 3) {
      //     for(auto k = 2; k < frames[i].size() - 2; k++) {
      //       chk_sum += data[k];
      //     }

      //     processing(frames[i].data(), frames[i].size());
      //    }
      // }
      // for (auto i = 0; i < data_vector.size(); i++)
      //   printf("0x%02x ", data_vector[i]);
      // printf("\n");

      frames.clear();
      // printf("after frame.size = %lu\n", frames.size());
      // printf("process  data_vector.size = %ld\n", data_vector.size());
    }
  }
}

// 00 00 00 00 00 2a 00 00
// eb 90 23 02 00 d1 02 37
// b9 00 00 00 80 ec 51 b8   2c b0 62 27 3f cc fe 80
// 3f b9 4a 1a 41 00 00 00   00 09 00 00 00 2a 00 1b
void RecvThread() {
  uint8_t rcvdat;
  int state_machine = 0;
  std::vector<uint8_t> data;
  while (1) {
    int len = read(fd, &rcvdat, 1);
    // printf("%d \n", len);
    if (len == 1) {
      if (state_machine == 0) {      // 协议解析状态机
          if (rcvdat == 0xeb)  {     // 接收到帧头第一个数据
              state_machine = 1;
              data.emplace_back(rcvdat);
          } else {
              state_machine = 0;      // 状态机复位
          }
      } else if (state_machine == 1) {
          if (rcvdat == 0x90) {      // 接收到帧头第二个数据
              state_machine = 2;
              data.emplace_back(rcvdat);
          } else {
              state_machine = 0;      // 状态机复位
              data.clear();
          }
      } else if (state_machine == 2) {
          data.emplace_back(rcvdat);  // 确认帧头 0xeb 0x90 之后
          if (rcvdat == 0x2a) {   // 帧尾第一个数据
              state_machine = 3;
          }
      } else if (state_machine == 3) {
          if (rcvdat == 0x00) {  // 帧尾第二个数据
              state_machine = 4;
              data.emplace_back(rcvdat);
          } else {
              state_machine = 3;  // 第一个帧尾可能是数据
          }
      } else if (state_machine == 4) {
          data.emplace_back(rcvdat);  // 校验
          state_machine = 0;
          for (auto v : data) {
              printf("%02x ", v);
          }
          printf("\n");
          // processing(data.data(), data.size());
          data.clear();
      }
    } else {
      data.clear();
      state_machine = 0;
    }

  }
}

int main() {
  const char *def_dev = "/dev/ttyUSB0";
  int uart_fd = open(def_dev, O_RDWR | O_NOCTTY);
  if (uart_fd < 0) {
    printf("open %s failed\n", def_dev);
    exit(-1);
  }
  if (uart_init(uart_fd, 256000)) {
    printf("init uart: %s faild!\n", def_dev);
    exit(-1);
  }


  FILE *fid = nullptr;
  fid = fopen("binary.dat", "wb");
  if (fid == nullptr) {
      printf("写出文件出错\n");
  }

   std::thread th(&process, uart_fd);
   while (1) {
     usleep(1000);
   }
  #if 0
  FrameParser<std::uint8_t> parser{{ 0xeb, 0x90 }, { 0x2a, 0x00 }, false};
  std::vector<std::vector<std::uint8_t>> frames;
  while (1) {
    uint8_t tmp_buf[BUFSIZE];

    int uart_len = read(uart_fd, tmp_buf, BUFSIZE);
    if (uart_len < 0) {
        printf("read uart failed\n");
        exit(1);
    }
    if (uart_len != 0) {
      for (int i = 0; i < uart_len; i++) {
        data_vector.emplace_back(tmp_buf[i]);
      }
      printf("data_vector.size = %ld\n", data_vector.size());

      /*if (fid != nullptr) {
        fwrite(tmp_buf, 1, uart_len, fid);
      } else {
        printf("failed to write file\n");
      }*/
      parser.append(data_vector);
      parser.parse(frames);
      // for (auto data : frames) {
      //   for (auto vv : data) {
      //       printf("0x%02x ", vv);
      //   }
      //   printf("\n");
      // }
      processing(frames.data()->data(), frames.data()->size());
      // std::cout << "frames nums = " << frames.size() << std::endl;
      // printf("%s \n", MSG.version);
      
      frames.clear();
      printf("process  data_vector.size = %ld\n", data_vector.size());
    }
  }
  #endif
  if (fid != nullptr) {
    fclose(fid);
  }

  return 0;
}
